#!/usr/bin/env python

""" test_pub.py - Version 0.1 2011-06-10  
"""

import roslib
roslib.load_manifest('pi_rein')
import rospy
import sys
import cv
from pi_rein.msg import *
from pi_rein.srv import *
from sensor_msgs.msg import Image, CameraInfo
from std_msgs.msg import Header, ColorRGBA
from cv_bridge import CvBridge, CvBridgeError

import random


class TestPub:
    def __init__(self):
        rospy.init_node("test_pub")
        
        self.test_pub = rospy.Publisher("random_markers", DetectionArray)

        self.cv_bridge = CvBridge()
        self.image_sub = rospy.Subscriber("image", Image, self.image_callback)
        
        rospy.wait_for_service('/display_image/draw_marker') 
        self.draw_marker = rospy.ServiceProxy('/display_image/draw_marker', DrawMarker)
        
        self.testbox_color = ColorRGBA(0, 255, 0, 0)
            
    def image_callback(self, ros_image):
        (self.image, self.image_size, self.image_width, self.image_height) = self.ros2cv_image(ros_image, "bgr8")

        self.image_stamp = ros_image.header.stamp
        self.testboxes = self.getDetections()
        #self.test_pub.publish(self.testboxes)
        for testbox in self.testboxes.detections:
            self.draw_marker(testbox.header, testbox.mask.roi, self.testbox_color, 0)
                
        rospy.sleep(0.1)
            
    def getDetections(self):
        faces = None
        detections = DetectionArray()
        detections.detections = []
        
        detection = Detection()
        detection.label = 'face_test'
        detection.detector = 'face_test'
        detection.header = Header()
        detection.mask = Mask()
        detection.mask.roi = Rect(random.randrange(0, self.image_width - 50), random.randrange(0, self.image_height - 50), 49, 49)
        detection.header.stamp = self.image_stamp
        detection.header.frame_id = '/openni_rgb_optical_frame'

        detections.detections.append(detection)
        
        detections.header.stamp = self.image_stamp
        detections.header.frame_id = '/openni_rgb_optical_frame'
    
        return detections
    
    def ros2cv_image(self, ros_image, type):
        try:
            cv_image = self.cv_bridge.imgmsg_to_cv(ros_image, type)
            size = cv.GetSize(cv_image)
            width = size[0]
            height = size[1]
            return (cv_image, size, width, height)
        except CvBridgeError, e:
          print e

def main(args):   
    try:
        TestPub()
        rospy.spin()
    except KeyboardInterrupt:
        print "Shutting down test pub node."

if __name__ == '__main__':
    main(sys.argv)
    